#!/usr/bin/env python3
# -*- encoding: utf-8 -*-
'''
@File    :   lidar_params_convert.py
@Time    :   2024/10/31 16:24:44
@Author  :   YcZ
@Version :   1.0
@Contact :   yuchao.zhou@outlook.com
@License :   (C)Copyright 2022-2024, YcZ
@Desc    :   
'''


import yaml
import argparse
import os

from camera_params_convert import dump_yaml, insert_, load_extrinsic_sample

def parse_and_convert_height(yaml_file, output_folder):
    data = None
    with open(yaml_file, 'r') as f:
        data = yaml.load(f, Loader=yaml.FullLoader)
    if data is None:
        print("\t", "Error: data is None")
        return
    dump_yaml(os.path.join(output_folder, 'lidar32_height.yaml'), data)


def parse_and_convert_lidar_extrinsic(yaml_file, output_folder):
    with open(yaml_file, 'r') as f:
        data = yaml.load(f, Loader=yaml.FullLoader)

    if "novatel" in yaml_file: # lidar to novatel extrinsics
        frame_id = "novatel"
    else: # lidar to lidar
        frame_id = "lidar32_top"

    filenames = os.path.basename(yaml_file).split('_')
    pos = filenames[1]
    if pos == 'back':
        child_frame_id = insert_(["lidar32", "top"])
    else:
        child_frame_id = insert_(["livox", pos])

    lidar_extrinsics = load_extrinsic_sample()
    lidar_extrinsics['header']['frame_id'] = frame_id
    lidar_extrinsics['child_frame_id'] = child_frame_id
    lidar_extrinsics['transform'] = data['transform']
    dump_yaml(os.path.join(output_folder, insert_(["lidar", child_frame_id, "to", frame_id, "extrinsics"])+'.yaml'), lidar_extrinsics)


if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument('-d','--directory', type=str, default='./orin_camera_params')
    parser.add_argument('-o','--output', type=str, default='./params_output3')
    args = parser.parse_args()

    folder = args.directory
    output_folder = args.output
    if not os.path.exists(output_folder):
        os.makedirs(output_folder)
    for file in os.listdir(folder):
        print("checking",file)
        if file.endswith('.yml') or file.endswith('.yaml'):
            if 'velodyne' in file:
                print("convert extrinsic", file)
                parse_and_convert_lidar_extrinsic(os.path.join(folder, file), output_folder)
            else:
                print("pass", file)
        else:
            continue
        # input()
    print("Done")

